#line 1 "D:/Archivio/mikroC/tric-ricotta/pid.c"
#line 1 "d:/archivio/mikroc/tric-ricotta/config.h"
#line 1 "d:/archivio/mikroc/tric-ricotta/pid.h"



void PID_update(float *RX_angle, float *SENSORS_angle, float *PID_output);
void PID_applyToMotors(float *PID_output, unsigned short *MOTORS_speed);
#line 11 "D:/Archivio/mikroC/tric-ricotta/pid.c"
void PID_update(float *RX_angle, float *SENSORS_angle, float *PID_output) {
 static float PID_err_proportional_old[3];
 static float PID_err_integral[3];
 float PID_err_proportional[3];
 float PID_err_derivative[3];
 float PID_output[3];
 unsigned short cnt;

 for (cnt=0; cnt<3; cnt++) {

 PID_err_proportional[cnt] = RX_angle[cnt] - SENSORS_angle[cnt];
 if((PID_err_proportional[cnt] >  0.01 ) || (PID_err_proportional[cnt] < - 0.01 )) PID_err_integral[cnt] += PID_err_proportional[cnt]*  0.002 ;
 PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) /  0.002 ;
 PID_output[cnt] = ( 0.7 *PID_err_proportional[cnt]) + ( 0.15 *PID_err_integral[cnt]) + ( -0.01 *PID_err_derivative[cnt]);

 if(PID_output[cnt] >  255 ) PID_output[cnt] =  255 ;
 else if(PID_output[cnt] <  0 ) PID_output[cnt] =  0 ;

 PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
 }
}

void PID_applyToMotors(float *PID_output, unsigned short *MOTORS_speed) {
 unsigned short cnt;
 int motors_temp;

 for (cnt=0; cnt<4; cnt++) {
 switch(cnt) {
 case 0: motors_temp = (int)(PID_output[0]*1.1547 + PID_output[1]*0.5); break;
 case 1: motors_temp = (int)(-(PID_output[0]*1.1547) + PID_output[1]*0.5); break;
 case 2: motors_temp = (int)(-(PID_output[1])); break;
 case 3: motors_temp = (int)(PID_output[2]); break;
 }
 motors_temp = (int)(PID_output[0]*1.1547 + PID_output[1]*0.5);
 if (motors_temp <  20 ) MOTORS_speed[cnt] =  20 ;
 else if (motors_temp >  250 ) MOTORS_speed[cnt] =  250 ;
 else MOTORS_speed[cnt] = (unsigned short)(motors_temp);
 }
}
